#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
from sensor_msgs.msg import Joy
import DrEmpower as dr

class GripperOpenController:
    def __init__(self):
        # 初始化节点
        rospy.init_node('gripper_open_controller', anonymous=True)
        rospy.loginfo("手柄控制夹爪节点已启动，等待按下按键 [6] 张开夹爪...")

        # 订阅 /joy 话题
        self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_callback)

        # 防止重复触发的标志
        self.last_button_state = 0  

        # 夹爪电机ID
        self.id_num = 7  

    def joy_callback(self, msg):
        """
        手柄回调函数：
        当 button[6] 从未按下 -> 按下 的瞬间执行夹爪张开
        """
        try:
            current_state = msg.buttons[6]
        except IndexError:
            rospy.logwarn("手柄按键数据异常：按钮索引超出范围")
            return

        # 检测上升沿（防止持续触发）
        if current_state == 1 and self.last_button_state == 0:
            rospy.loginfo("检测到按键 [6] 按下，执行夹爪张开动作...")
            self.open_gripper()

        self.last_button_state = current_state

    def open_gripper(self):
        """
        控制夹爪张开
        """
        try:
            dr.set_angle(id_num=self.id_num,
                         angle=-45,   # 夹爪张开角度
                         speed=5,
                         param=10,
                         mode=1)
            rospy.loginfo("✅ 夹爪已张开")
        except Exception as e:
            rospy.logerr(f"夹爪张开失败: {e}")

    def spin(self):
        rospy.spin()


if __name__ == '__main__':
    try:
        controller = GripperOpenController()
        controller.spin()
    except rospy.ROSInterruptException:
        rospy.logerr("ROS 中断异常")
